#include <iostream>
#include <chrono>
#include <opencv2/opencv.hpp>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;
double f(double xi,double nme,double n_e,double re){return nme/(1+((nme/n_e)-1)*exp(-re*(xi)));}
//double res(double xi){return f(xi,350.529,391.299);}
//double f(double xi,double nme,double re){return nme*xi+re;}
int main(int argc, char **argv) {
  double nme = 2000, re =0.4 ,n_e=0.5;        // 估计参数值
  int N = 19;                                 // 数据
  vector<double> x_data;      // 数据
  for (int i = 0; i < N; i++) {
    double x = i+1;
    x_data.push_back(x);  
  }
  vector<double> y_data{0,4,10,11,15,16,21,32,37,66,129,195,317,518,671,757,903,1049,1212};
  int iterations = 100;    // 迭代次数
  double cost = 0, lastCost = 0;  // 本次迭代的cost和上一次迭代的cost
  chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
  for (int iter = 0; iter < iterations; iter++) {
    Matrix3d H = 0.000000000000001*Matrix3d::Identity();             // Hessian = J^T W^{-1} J in Gauss-Newton
    Vector3d b = Vector3d::Zero();             // bias
    cost = 0;
    for (int i = 0; i < N; i++) {
      double xi = x_data[i], yi = y_data[i];  // 第i个数据点
      double error = yi - f(xi,nme,n_e,re);
      double eps = 1e-9;
      Vector3d J; // 雅可比矩阵
      J[0] =  -(f(xi,nme+eps,n_e,re)-f(xi,nme,n_e,re))/eps;// de/da
      J[1] = -(f(xi,nme,n_e+eps,re)-f(xi,nme,n_e,re))/eps;
      J[2] = -(f(xi,nme,n_e,re+eps)-f(xi,nme,n_e,re))/eps;  // de/dc
      //std::cout<<"J:"<<J<<std::endl;
      H +=  J * J.transpose();
      b -=  error * J;
      cost += error * error;
    }
    std::cout<<"H:"<<H<<"\n b:"<< b <<std::endl;
    // 求解线性方程 Hx=b
    Vector3d dx = H.ldlt().solve(b);
    std::cout<<"dx:"<<dx;
    if (isnan(dx[0])) {
      cout << "result is nan!" << endl;
      break;
    }
  nme += dx[0];
  n_e += dx[1];
  re  += dx[2];
  lastCost = cost;
  cout << "total cost: " << cost << ", \t\tupdate: " << dx.transpose() <<
         "\t\testimated params: " << nme << "," << re << endl;
  }
  chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
  chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2 - t1);
  cout << "solve time cost = " << time_used.count() << " seconds. " << endl;
  cout << "estimated nm n0 r = " << nme << ", " << n_e << ", " << re << endl;
  return 0;
}